import rclpy
from .ros2_driver import ROS2Driver
from .http_server import Server
class Driver:
    '''
        控制器
    '''
    def __init__(self):
        '''
            初始化
        '''
        self.LoadCore()
    
    def LoadCore(self):
        '''
            核心资源
        '''
        #   ROS2 接口
        self.ros2_driver=ROS2Driver()
        #   GPS通信接口
        if self.ros2_driver.data_type=='uart': 
            print('[提示] 绑定设备号 {},通信类型 {},RTK坐标系名称 {},设备串口号 {}'.format(self.ros2_driver.dev_id,self.ros2_driver.data_type,self.ros2_driver.frame_id,self.ros2_driver.uart_port))
            from .uart_driver import RTKDriver
            self.rtk_driver=RTKDriver(port=self.ros2_driver.uart_port,buffer_len=self.ros2_driver.buffer_len,callback=self.Publish)
        elif self.ros2_driver.data_type=='wifi':
            print('[提示] 绑定设备号 {},通信类型 {},RTK坐标系名称 {}'.format(self.ros2_driver.dev_id,self.ros2_driver.data_type,self.ros2_driver.frame_id))
            from .udp_driver import RTKDriver
            self.rtk_driver=RTKDriver(port=8890,buffer_len=self.ros2_driver.buffer_len,callback=self.Publish)
        else:
            print("参数错误，程序将退出")
            exit(-1)
        print("[提示] 如果需要修改参数，请在/config/config.yaml文件中设置！")
        #   HTTP服务器接口 
        if self.ros2_driver.http_port>0:
            self.http_server=Server(rtk_driver=self.rtk_driver,port=self.ros2_driver.http_port)
        
        
    def Publish(self):
        '''发布数据'''
        dev_id=self.ros2_driver.dev_id
        if not dev_id in self.rtk_driver.buffer.keys(): return
        if len(self.rtk_driver.buffer[dev_id])<1: return
        data=self.rtk_driver.buffer[dev_id][-1]
        self.ros2_driver.Publish(x=data['XYZ'][0],y=data['XYZ'][1],z=data['XYZ'][2],
                                 lon=data['LLA'][0],lat=data['LLA'][1],height=data['LLA'][2],
                                 speed=data['SPD'],yaw=data['DIR'],rms=data['RMS'],status=data['MODE'])
def main(args=None):
    rclpy.init(args=args)
    driver=Driver()
    rclpy.spin(driver.ros2_driver)
    driver.destroy_node()
    driver.shutdown()